/** ###################################################################
**     Filename  : Accel.C
**     Project   : project_xgate
**     Processor : MC9S12XDP512BMPV
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 3/25/2009, 2:53 AM
**     Contents  :
**         User source code
**
**     (c) Copyright UNIS, a.s. 1997-2008
**     UNIS, a.s.
**     Jundrovska 33
**     624 00 Brno
**     Czech Republic
**     http      : www.processorexpert.com
**     mail      : info@processorexpert.com
** ###################################################################*/

/* MODULE Accel */
#include "Accel.h"
#include "Accel_Fifo.h"
#include "RunLoad.h"
#include "Button.h"
#include "IEE1.h"

#define DEBUG 1

#if DEBUG
  #include "X_Val.h"
  #include "Y_Val.h"
#endif



#define NUM2AVG 1000 //do not exceed 65k, use 30k for average


//AVG is computed when the accelerometer is at rest and 30k averages are taken
//MIN and MAX are computed with a 5000 average window and 1g force is applied (accelerometer rotated to different gravity field orientation)
unsigned short Xavg;// 4809
unsigned short Xmin;// 3616
unsigned short Xmax;// 5965

unsigned short Yavg;// 5134
unsigned short Ymin;// 3962
unsigned short Ymax;// 6276

//1g is a compile time calc based on experimental data input above
unsigned short X1g;// ((Xmax-Xmin)/2)
unsigned short Y1g;// ((Ymax-Ymin)/2)

word lostx,losty;//shows lost fifo data

/*Interrupt Process Function*/
word Num2Avg = NUM2AVG; 

void Accel_Init(void){
  IEE1_TAddress calibration_data;
  
  Accel_Fifo_Init();
  
  Inhr1_Enable(); 
  Inhr1_Reset();
  
  Accel_Y_Enable();
  Accel_Y_Reset();
  
  calibration_data = IEE1_AREA_START;
       
  IEE1_GetWord(calibration_data,&Xmin);
  IEE1_GetWord(calibration_data+1,&Xmax);
  IEE1_GetWord(calibration_data+2,&Xavg);
  IEE1_GetWord(calibration_data+3,&X1g);
  
  IEE1_GetWord(calibration_data+4,&Ymin);
  IEE1_GetWord(calibration_data+5,&Ymax);
  IEE1_GetWord(calibration_data+6,&Yavg);
  IEE1_GetWord(calibration_data+7,&Y1g);
  
}


void Inhr1_Process(void){
  static word count;
  static long highsum,lowsum; 
  Accel_TData xdata;
  
  if(!X_GetVal()){//latch on falling edge
    Inhr1_Latch();                           
  }else{//calculate pulse width on rising edge
    Inhr1_GetHoldValue((Inhr1_TCapturedValue *)&xdata.hightime);
    Inhr1_GetCaptureValue((Inhr1_TCapturedValue *)&xdata.lowtime);
    Inhr1_Reset();
    
    if(NUM2AVG == count){               
      xdata.source    = ACCELX;
      xdata.hightime  = (word)(highsum/Num2Avg);
      xdata.lowtime   = (word)(lowsum/Num2Avg);    
      
      if(Accel_Fifo_Put(xdata))
        lostx++;//failed to put, lost one
        
      count = highsum = lowsum = 0;     

  #if DEBUG    
      X_Val_NegVal();
  #endif
  
    }else{
      highsum += xdata.hightime;
      lowsum  += xdata.lowtime;
      count++;
    }
  }
}

/*Interrupt Process Function*/

void Accel_Y_Process(void){
  static word count;
  static long highsum,lowsum; 
  Accel_TData ydata;
  
  if(!Y_GetVal()){//latch on falling edge
    Accel_Y_Latch();                           
  }else{//calculate pulse width on rising edge
    Accel_Y_GetHoldValue((Accel_Y_TCapturedValue *)&ydata.hightime);
    Accel_Y_GetCaptureValue((Accel_Y_TCapturedValue *)&ydata.lowtime);
    Accel_Y_Reset();    
    if(NUM2AVG == count){               
    ydata.source    = ACCELY;
    ydata.hightime  = (word)(highsum/Num2Avg);
    ydata.lowtime   = (word)(lowsum/Num2Avg);    
    
    if(Accel_Fifo_Put(ydata)) 
      losty++;//failed to put, lost one
      
    count = highsum = lowsum = 0;     

#if DEBUG    
      Y_Val_NegVal();
#endif    
    }else{
      highsum += ydata.hightime;
      lowsum  += ydata.lowtime;
      count++;
    }
  }
}


/* convert raw data into duty cycle */
word Accel_GetDuty(Accel_TData data){
   return ((10000*(long)data.hightime/data.lowtime));
}

/* convert duty cycle into g's of acceleration*/
signed short Accel_GetAccelX(word xduty){
  return (signed short)(((signed long)xduty-Xavg)*1000/X1g);
}

/* convert duty cycle into g's of acceleration*/
signed short Accel_GetAccelY(word yduty){
  return (signed short)(((signed long)yduty-Yavg)*1000/Y1g);  
}

#include <math.h>
#define STEADYTHRESHOLD 50
/* takes control of LCD and instructs the user for calibration input */      
word getCalVal(byte dir){
  byte steadycountdown = 100;
  Accel_TData data;
  word dx,dy;
  word lastdx,lastdy;  
  IEE1_TAddress calibration_data;
  char i;
  
  calibration_data = IEE1_AREA_START;
  //instruct the user, keep showing until orientation steadies for 100 samples
  LCD_Clear(0);
  LCD_GoTo(2,1);LCD_OutString("Face up->toggle sw");
  //LCD_OutDec(dir*90); LCD_OutString(" down");
  while(Button_GetVal());
  while(!Button_GetVal());
  do{
    if(!Accel_Fifo_Get(&data)){
      if(data.source==ACCELX){
          lastdx = dx;          
          dx = Accel_GetDuty(data);
          LCD_GoTo(6,1);LCD_OutString("x = ");LCD_OutFix2(dx);LCD_OutChar('%');          
      }else{
          lastdy = dy;
          dy = Accel_GetDuty(data);
          LCD_GoTo(6,10);LCD_OutString("y = ");LCD_OutFix2(dy);LCD_OutChar('%');          
      }
    }    
  if(abs(lastdx-dx)<STEADYTHRESHOLD && abs(lastdy-dy)<STEADYTHRESHOLD) /*detect steady*/
    steadycountdown--;
  }while(steadycountdown);
 
  //getting Zero Gravity point;
  Xavg = dx;
  Yavg = dy;
  Xmin = dx;
  Xmax = dx;
  Ymin = dy;
  Ymax = dy;
  
    for(i =0;i <4;i++) {
      
      LCD_GoTo(2+i,1);LCD_OutString("Side 1->toggle swi");
        while(Button_GetVal());
         while(!Button_GetVal());
       steadycountdown = 100;
      do{
        if(!Accel_Fifo_Get(&data)){
          if(data.source==ACCELX){
              lastdx = dx;          
              dx = Accel_GetDuty(data);
              LCD_GoTo(6,1);LCD_OutString("x = ");LCD_OutFix2(dx);LCD_OutChar('%');          
          }else{
              lastdy = dy;
              dy = Accel_GetDuty(data);
              LCD_GoTo(6,10);LCD_OutString("y = ");LCD_OutFix2(dy);LCD_OutChar('%');          
          }
        }    
      if(abs(lastdx-dx)<STEADYTHRESHOLD && abs(lastdy-dy)<STEADYTHRESHOLD) /*detect steady*/
        steadycountdown--;
      }while(steadycountdown);
        Xmin = (Xmin<=dx)?Xmin:dx;
        Xmax =  (Xmax>=dx)?Xmax:dx;
        Ymin = (Ymin<=dy)?Ymin:dy;
        Ymax = (Ymax>=dy)?Ymax:dy;
      
    }
    
    

  PTP_PTP7 = 1;
  LCD_GoTo(7,1);LCD_OutString("Success, start");
  X1g=((Xmax-Xmin)/2); 
  Y1g =((Ymax-Ymin)/2);
    
    IEE1_SetWord(calibration_data   ,Xmin);
    IEE1_SetWord(calibration_data+1 ,Xmax);
    IEE1_SetWord(calibration_data+2 ,Xavg);
    IEE1_SetWord(calibration_data+3 ,X1g);
                                  
    IEE1_SetWord(calibration_data+4 ,Ymin);
    IEE1_SetWord(calibration_data+5,Ymax);
    IEE1_SetWord(calibration_data+6,Yavg);
    IEE1_SetWord(calibration_data+7,Y1g);
     while(Button_GetVal());
     while(!Button_GetVal());
    LCD_Clear(0);
      //instruct to keep steady, calibrating
      //stop accelerometer
      //flush the buffer
      //set long average time        
      //reset accelerometer process variables
      //start accelerometer
      //wait for result
      //store to max in 0 direction  
      
}

#define DEG0    0 
#define DEG90   1
#define DEG180  2
#define DEG270  3

/* calibration algorithm*/
byte Accel_Calibrate(void){
  byte state;
  word calvals[4];
  DDRT_DDRT7 = 1; 
  
  switch(state){
    PTP_PTP7 = 0;      
    case DEG0:      
      calvals[state] = getCalVal(state);      
    break;
    case DEG90:      
      calvals[state] = getCalVal(state);      
    break;   
  }
  
  
  return ERR_NOISE;//failed to calibrate, too much "noise"
  
  return ERR_OK;
}


/* END Accel */
